#include "led_flow_task.h"
#include "cmsis_os.h"
#include "main.h"
#include "mitsuko.h"
#include "bsp_can.h"
#include "pid.h"
#include "usart.h"

int8_t P1 = 0;
int8_t P2 = 0;
int8_t P3 = 0;
int8_t P4 = 0;
int8_t P5 = 0;
int8_t P6 = 0;
int8_t P7 = 0;
int8_t P8 = 0;
int8_t P9 = 0;

int8_t B1 = 0;
int8_t B2 = 0;
int8_t B3 = 0;
int8_t B4 = 0;
int8_t B5 = 0;

int8_t W1 = 0;
int8_t W2 = 0;
int8_t W3 = 0;
int8_t W4 = 0;
int8_t W5 = 0;

int8_t S1 = 0;
int8_t S2 = 0;
int8_t S3 = 0;
int8_t S4 = 0;
int8_t S5 = 0;
int8_t S6 = 0;

extern int8_t Move_Temp_x;
extern int8_t Move_Temp_y;
extern CAN_HandleTypeDef hcan1;
extern Motor_measure_t  Motor_measure[14];
extern float text_x;
extern float text_y;
extern PID_typedef 			Motor_pid[14];
extern int8_t step;
extern int8_t process;
extern int8_t process_chess;
extern int8_t process_pos;

void led_RGB_flow_task(void const * argument)
{
	HAL_GPIO_WritePin(LED_RED_GPIO_Port,LED_RED_Pin,GPIO_PIN_RESET);
	HAL_GPIO_WritePin(LED_GREEN_GPIO_Port,LED_GREEN_Pin,GPIO_PIN_RESET);
	for(;;){
		HAL_GPIO_TogglePin(LED_GREEN_GPIO_Port,LED_GREEN_Pin);
		P1 = HAL_GPIO_ReadPin(GPIOF,GPIO_PIN_1);
		P2 = HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_5);
		P3 = HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_6);
		P4 = HAL_GPIO_ReadPin(GPIOC,GPIO_PIN_2);
		P5 = HAL_GPIO_ReadPin(GPIOC,GPIO_PIN_3);
		P6 = HAL_GPIO_ReadPin(GPIOC,GPIO_PIN_4);
		P7 = HAL_GPIO_ReadPin(GPIOC,GPIO_PIN_5);
		P8 = HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_5);
		P9 = HAL_GPIO_ReadPin(GPIOF,GPIO_PIN_10);
		
		B1 = HAL_GPIO_ReadPin(GPIOI,GPIO_PIN_7);
		B2 = HAL_GPIO_ReadPin(GPIOI,GPIO_PIN_6);
		B3 = HAL_GPIO_ReadPin(GPIOI,GPIO_PIN_5);
		B4 = HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_3);
		B5 = HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_2);
		
		W1 = HAL_GPIO_ReadPin(GPIOI,GPIO_PIN_0);
		W2 = HAL_GPIO_ReadPin(GPIOH,GPIO_PIN_12);
		W3 = HAL_GPIO_ReadPin(GPIOH,GPIO_PIN_11);
		W4 = HAL_GPIO_ReadPin(GPIOH,GPIO_PIN_10);
		W5 = HAL_GPIO_ReadPin(GPIOD,GPIO_PIN_15);
		
		S1 = HAL_GPIO_ReadPin(GPIOF,GPIO_PIN_0);
		S2 = HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_4);
		S3 = HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_12);
		S4 = HAL_GPIO_ReadPin(GPIOB,GPIO_PIN_0);
		S5 = HAL_GPIO_ReadPin(GPIOB,GPIO_PIN_1);
		S6 = HAL_GPIO_ReadPin(GPIOC,GPIO_PIN_0);
		
		if(Move_Temp_x == 1&&Move_Temp_y == 1)
		{
			Motor_measure[0].Output = PID_calc(&Motor_pid[0],Motor_measure[0].speed,text_x);
			Motor_measure[1].Output = PID_calc(&Motor_pid[1],Motor_measure[1].speed,text_y);
		}
		if(HAL_GPIO_ReadPin(GPIOB,GPIO_PIN_2) ==1)
		{
			step = 1;process = 0;
		}
		Set_motor_cmd(&hcan1,First_STDID,Motor_measure[0].Output,Motor_measure[1].Output,Motor_measure[2].Output,0);
		osDelay(1);
	}
}



















